%% Saturn
warning off
clc;clear;
format longg
options_negx = odeset('RelTol',1e-13,'AbsTol',1e-13,'Events',@NegXcrossing);
options=odeset('RelTol',1e-13,'AbsTol',1e-13);
options_fmincon = optimoptions('fmincon', 'MaxFunctionEvaluations', 5000, ...
    'MaxIterations', 5000,'ConstraintTolerance',1e-6,'Algorithm','Interior-point','stepTolerance',1e-21);
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons

thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.


planetNumb = 1;
[L1,L2,L3,L4,L5] = librationPoints(u(planetNumb));
L_ = [L1,L2,L3,L4,L5];
for ii = 1:length(L_)
    L_pos = L_(:,ii);
    J_L(ii) = jacobiConst(L_pos,zeros(3,1),u(planetNumb));
end

%% environment settings 
dt = 0.01;
startphase=0; % changing the starting position of the periodic
% orbit to show changes in the pole visibility based on the Satellite's position wrt the Sun's pole
rotationAxis = -7.25;
T1 = Rotation(cross([0;0;1],L_(:,4)),rotationAxis,'Degrees');
% OpenCRTBP_u(u)
%% Define inertial frame FNs
[xs,ys,zs] = ellipsoid(-0,0,0,696340,696340,696340,30);
s = surface(xs,ys,zs,'EdgeColor','flat','FaceAlpha',0.3,'FaceColor','k'); hold on;
axis equal
FN_temp = s.VertexNormals;
[row,col,depth] = size(FN_temp);
FN_i = zeros(size(FN_temp));
for ii = 1:row
    for jj = 1:col
        FN_i(ii,jj,:)=T1*[FN_temp(ii,jj,1);...
            FN_temp(ii,jj,2);...
            FN_temp(ii,jj,3)];
    end
end

%%
p1 = [0 0 0];                         % First Point
p2 = [0 0 1e6];                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'k','LineWidth',2)
text(p2(1),p2(2),p2(3), 'Z','FontSize',15,'BackgroundColor','none')

axis equal
p1 = [0 0 0];                         % First Point
p2 = [1e6 0 0];                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'k','LineWidth',2)
text(p2(1),p2(2),p2(3), 'X <Vernal Equinox>','FontSize',15,'BackgroundColor','none')

p1 = [0 0 0];                         % First Point
p2 = [0 1e6 0];                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'k','LineWidth',2)
text(p2(1),p2(2),p2(3), 'Y','FontSize',15,'BackgroundColor','none')


%%
rotationAxis = -7.25;
T1 = Rotation([1;0;0],rotationAxis,'Degrees');
rotationAxis = 73.67+0.014*(2023-1850);
T2 = Rotation([0;0;1],rotationAxis,'Degrees');
T = T2*T1;

p1 = [0 0 0];                         % First Point
p2 = T*[0 0 1e6]';                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'r','LineWidth',2)
text(p2(1),p2(2),p2(3), '+Z (North pole)','FontSize',15,'BackgroundColor','none','Color','r')

p1 = [0 0 0];                         % First Point
p2 = T*[0 0 1e6]';                         % Second Point
dp = 5*(p2'-p1);                         % Difference
% quiver3(p1(1),p1(2),0,dp(1),dp(2),0,'b','LineWidth',2)
% text(p2(1),p2(2),p2(3), '+Z (North pole)','FontSize',15,'BackgroundColor','w','Color','b')

p1 = [0 0 0];                         % First Point
p2 = T*[0 0 -1e6]';                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'r','LineWidth',2)
text(p2(1),p2(2),p2(3), '-Z (South pole)','FontSize',15,'BackgroundColor','none','Color','r')

axis equal
p1 = [0 0 0];                         % First Point
p2 = T*[1e6 0 0]';                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'r','LineWidth',2)
text(p2(1),p2(2),p2(3), 'X','FontSize',15,'BackgroundColor','none','Color','r')
 
p1 = [0 0 0];                         % First Point
p2 = T*[0 -1e6 0]';                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'r','LineWidth',2)
text(p2(1),p2(2),p2(3), '-Y','FontSize',15,'BackgroundColor','none','Color','r')

%%

p1 = [0 0 0];                         % First Point
p2 = T*[0 0 1e6]';                         % Second Point
dp = 9*(p2'-p1);                         % Difference
quiver3(p1(1),p1(2),0,dp(1),dp(2),0,'r--','LineWidth',6)
text(9*p2(1),9*p2(2),0, 'Pr(e)POLE','FontSize',15,'BackgroundColor','none','Color','r')

p1 = [0 0 0];                         % First Point
p2 = T*[1e6 1e6 1e6]';                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'g','LineWidth',5)
text(p2(1),p2(2),p2(3), 'SATELLITE','FontSize',15,'BackgroundColor','none','Color','g')

p1 = [0 0 0];                         % First Point
p2 = T*[1e6 1e6 1e6]';                         % Second Point
dp = (p2-p1);                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),0,'g','LineWidth',5,'LineStyle','--')
text(p2(1),p2(2),0, 'Pr(e)SATELLITE','FontSize',15,'BackgroundColor','none','Color','g','LineStyle','--')

% p1 = [0 0 0];                         % First Point
% p2 = T*[0 1e6 1e6]';                         % Second Point
% dp = p2-p1;                         % Difference
% quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'g','LineWidth',5)
% text(p2(1),p2(2),p2(3), 'SATELLITE','FontSize',15,'BackgroundColor','none','Color','g')
% 
% p1 = [0 0 0];                         % First Point
% p2 = T*[0 1e6 1e6]';                         % Second Point
% dp = (p2-p1);                         % Difference
% quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),0,'g','LineWidth',5,'LineStyle','--')
% text(p2(1),p2(2),0, 'Pr(e)SATELLITE','FontSize',15,'BackgroundColor','none','Color','g','LineStyle','--')

[X,Y] = meshgrid(-1e6:100000:1e6);
mesh(X,Y,zeros(size(X)),'LineStyle','-','FaceAlpha',0.5)